#include "rootNode.h"
#include "mat.h"
#include <matrix.h>
#include <highgui.h>
#include "subAR.h"

#include <opencv2/core/core.hpp>

#include <memory.h>


bool rootNode::loadImageToModel(const char*file_name,const char*prefix){
	char mat_file_name[500];
	sprintf(&mat_file_name[0],"%s_%smaps.mat",file_name,prefix);
	if (!image.readDataFromMatFile(mat_file_name)){
		printf("Failed to load data from file %s,\n",mat_file_name);
		return false;
	}
	/*update image data reference of it's children nodes*/
	for(int i =0; i<tan_OR_list.size();++i) tan_OR_list[i].updateImageDataRef(&image);
	for(int i = 0; i<vol_AND_list.size(); ++i) vol_AND_list[i].updateImageDataRef(&image);
	for(int i = 0; i<vol_OR_list.size(); ++i) vol_OR_list[i].updateImageDataRef(&image);
	return true;
}


bool rootNode::inference(const char *file_name){
	/*initilailze current image file*/
	char mat_file_name[512];
	this->loadImageToModel(file_name);

	/*read line pair data*/
	sprintf(&mat_file_name[0],"%sall_curve.mat",file_name);
	MATFile *p_file =matOpen(mat_file_name,"r");
	mxArray *p_curve = matGetVariable(p_file,"para_lines");
	double *p_para_line = mxGetPr(p_curve);
	int n_line = mxGetM(p_curve);
	int n_col = mxGetN(p_curve);

	/*perfrom inference*/
	//for( int i_node = 0; i_node<this->tan_OR_list.size(); ++i_node){
	for( int i_node = 0; i_node<1; ++i_node){
		for( int i_line = 0 ; i_line<n_line; i_line+=2){
			line mat_line_1,mat_line_2;
			mat_line_1.orient=ROUND(p_para_line[i_line]);
			mat_line_1.length = ROUND(p_para_line[i_line + 2*n_line]);
			mat_line_1.x = ROUND(p_para_line[i_line + 3*n_line]);
			mat_line_1.y = ROUND(p_para_line[i_line + 4*n_line]);

			mat_line_2.orient=ROUND(p_para_line[i_line+1]);
			mat_line_2.length = ROUND(p_para_line[i_line+1 + 2*n_line]);
			mat_line_2.x = ROUND(p_para_line[i_line +1 + 3*n_line]);
			mat_line_2.y = ROUND(p_para_line[i_line +1 + 4*n_line]);


			tan_OR_list[i_node].inference(mat_line_1,mat_line_2);
		}


	}
	mxDestroyArray(p_curve);
	matClose(p_file);

	window.setBackground(image.sy,image.sx,image.img_r,image.img_g,image.img_b);
	/*

	for( int i_node = 0; i_node<this->tan_OR_list.size(); ++i_node){
	int n_ptc = tan_OR_list[i_node].tan_particle_list.size();
	std::list<tanParticle_s>::iterator itr;
	for( itr=tan_OR_list[i_node].tan_particle_list.begin(); 
	itr!=tan_OR_list[i_node].tan_particle_list.end(); ++itr){
	window.drawLinePairs(itr->pt_pairs,7,2);
	}
	}

	*/


	/*update the tan particles in each tan, by arg_max2*/
	for( int i_node = 0; i_node<this->tan_OR_list.size(); ++i_node){
		int n_ptc = tan_OR_list[i_node].tan_particle_list.size();
		std::list<tanParticle_s>::iterator itr;
		for( itr=tan_OR_list[i_node].tan_particle_list.begin(); 
			itr!=tan_OR_list[i_node].tan_particle_list.end(); ++itr){
				tan_OR_list[i_node].argMax2(itr->P,itr->cam_dir,itr->arg_max_deformation,*itr);
		}
	}



	/*visualize the top few lines in each tan node*/
	//window.setBackground(image.sy,image.sx,image.img_r,image.img_g,image.img_b);


	for( int i_node = 0; i_node<this->tan_OR_list.size(); ++i_node){
		int ipa = 0;
		int n_ptc = tan_OR_list[i_node].tan_particle_list.size();
		std::list<tanParticle_s>::iterator itr;
		for( itr=tan_OR_list[i_node].tan_particle_list.begin(); 
			itr!=tan_OR_list[i_node].tan_particle_list.end(); ++itr){
				window.cleanImage();
				window.drawLinePairs(itr->pt_pairs,ipa+10,2);
				window.drawP(itr->P);
				for( int i =0; i<tan_OR_list.size();++i)
					window.drawTanTemplate(tan_OR_list[i],itr->P,itr->cam_dir,5);	
				tan_OR_list[i_node].argMax2(itr->P,itr->cam_dir,itr->arg_max_deformation,*itr);
				ipa++;
		}
	}


	/*performe inference in volAND node*/
	for(int i_node = 0; i_node<this->vol_AND_list.size(); ++i_node){
		vol_AND_list[i_node].inference();
		int n_ptc = vol_AND_list[i_node].vol_particle_list.size();
		std::list<volParticle_s>::iterator itr = vol_AND_list[i_node].vol_particle_list.begin();
		for(; itr!=vol_AND_list[i_node].vol_particle_list.end(); ++itr){
			vol_AND_list[i_node].argMax(*itr);
		}
	}


	/*inference and in volOR node*/
	for(int i_node = 0; i_node<this->vol_OR_list.size(); ++i_node){
		vol_OR_list[i_node].inference();
		//int n_ptc = vol_OR_list[i_node].vol_particle_list.size();
		//std::list<volParticle_s>::iterator itr = vol_OR_list[i_node].vol_particle_list.begin();
		//for(; itr!=vol_OR_list[i_node].vol_particle_list.end(); ++itr){
		//	vol_OR_list[i_node].argMax(*itr);
		//}
	}

	/*inference in root node*/
	for( int i_node = 0; i_node<this->vol_OR_list.size(); ++i_node){
		int n_ptc = vol_OR_list[i_node].vol_particle_list.size();
		std::list<volParticle_s>::iterator itr = vol_OR_list[i_node].vol_particle_list.begin();
		for(; itr!=vol_OR_list[i_node].vol_particle_list.end(); ++itr){
			rootParticle_s particle;
			this->test(itr->P,itr->cam_dir,particle);

			//for(int row = 0; row<3; ++row){
			//	memcpy(particle.P[row],itr->P[row],sizeof(float)*4);
			//}
			//memcpy(particle.cam_dir,itr->cam_dir,sizeof(float)*3);
			this->updateParticleList(particle);
		}
	}




	std::list<rootParticle_s>::iterator itr=root_particle_list.begin();
	std::list<rootParticle_s>::iterator itr_end=root_particle_list.end();
	int ipa=0;
	//for( itr=root_particle_list.begin(); 
	//	itr!=root_particle_list.end(); ++itr){
	for( ; ipa<PR_QUEUE_SIZE; ){
		this->argMax(*itr);
		window.cleanImage();
		window.drawRootPtc(*itr);
		window.drawP(itr->P);
		rootParticle_s ptc;
		this->lookAround(itr->P,ptc);
		window.cleanImage();
		window.drawRootPtc(ptc);
		window.drawP(ptc.P);
		//this->updateParticleList(ptc);
		this->root_particle_list.push_back(ptc);
		ipa++;
	}
	this->root_particle_list.sort();

	ipa=0;
	for( itr=root_particle_list.begin(); 
		itr!=root_particle_list.end(); ++itr){
			this->argMax(*itr);
			window.cleanImage();
			window.drawRootPtc(*itr);
			window.drawP(itr->P);
			ipa++;
	}





	/*release image data*/
	image.release();

	return true;
}


/*
* Need to be re-written soon
*/

bool rootNode::inferenceV2(const char *file_name){
	/*initilailze current image file*/
	char mat_file_name[512];
	sprintf(&mat_file_name[0],"%s_maps.mat",file_name);
	if (!image.readDataFromMatFile(mat_file_name)){
		printf("Failed to load data from file %s,\n",mat_file_name);
		return false;
	}
	/*update image data reference of it's children nodes*/
	/*update image data reference of it's children nodes*/
	for(int i =0; i<tan_OR_list.size();++i) tan_OR_list[i].updateImageDataRef(&image);
	for(int i = 0; i<vol_AND_list.size(); ++i) vol_AND_list[i].updateImageDataRef(&image);
	for(int i = 0; i<vol_OR_list.size(); ++i) vol_OR_list[i].updateImageDataRef(&image);
	/*read line pair data*/
	sprintf(&mat_file_name[0],"%sall_curve.mat",file_name);
	MATFile *p_file =matOpen(mat_file_name,"r");
	mxArray *p_curve = matGetVariable(p_file,"para_lines");
	double *p_para_line = mxGetPr(p_curve);
	int n_line = mxGetM(p_curve);
	int n_col = mxGetN(p_curve);
	/*perfrom inference*/
	float distThresh = std::min(image.sx,image.sy)/3;
	distThresh *=distThresh;
	//for( int i_node = 0; i_node<this->tan_OR_list.size(); ++i_node){
	for( int i_node = 1; i_node<2; ++i_node){
		for( int i_line = 0 ; i_line<n_line; i_line+=2){

			line mat_line_1,mat_line_2;
			mat_line_1.orient=ROUND(p_para_line[i_line]);
			mat_line_1.length = ROUND(p_para_line[i_line + 2*n_line]);
			mat_line_1.x = ROUND(p_para_line[i_line + 3*n_line]);
			mat_line_1.y = ROUND(p_para_line[i_line + 4*n_line]);

			mat_line_2.orient=ROUND(p_para_line[i_line+1]);
			mat_line_2.length = ROUND(p_para_line[i_line+1 + 2*n_line]);
			mat_line_2.x = ROUND(p_para_line[i_line +1 + 3*n_line]);
			mat_line_2.y = ROUND(p_para_line[i_line +1 + 4*n_line]);


			/*check if line pair is too far away*/
			float dist = (mat_line_1.x-mat_line_2.x)*(mat_line_1.x-mat_line_2.x);
			dist += (mat_line_1.y-mat_line_2.y)*(mat_line_1.y-mat_line_2.y);
			if( dist>distThresh)
				continue;

			vol_AND_list[i_node].inferenceV2(mat_line_1,mat_line_2);
		}


	}
	mxDestroyArray(p_curve);
	matClose(p_file);

	window.setBackground(image.sy,image.sx,image.img_r,image.img_g,image.img_b);

	for(int i_vol = 0; i_vol<this->vol_AND_list.size(); ++i_vol){
		volAND & vol = vol_AND_list[i_vol];
		std::list<volParticle_s>::iterator itr = vol.vol_particle_list.begin();
		for( ;itr!=vol.vol_particle_list.end(); ++itr){
			window.cleanImage();
			window.drawVolPtc(*itr,20);
			window.drawP(itr->P);
		}
	}



	/*release image data*/
	image.release();

	return true;
}



void rootNode::argMax(rootParticle_s &particle, const char* template_name){	
	float* p_template = NULL;
	if(template_name !=NULL){
		p_template =(float*) malloc(image.sx*image.sy*sizeof(*p_template));
		memset(p_template,0,image.sx*image.sy*sizeof(float));
	}



	/*go back to retrieve the point correspondence*/
	particle.score = 0;
	assert(particle.vol_particle_list.size()==this->vol_OR_list.size());
	for(int i_node = 0; i_node<particle.vol_particle_list.size();++i_node){
		this->vol_OR_list[i_node].argMax(particle.vol_particle_list[i_node],
			particle.or_index_list[i_node],
			p_template);
		particle.score +=particle.vol_particle_list[i_node].score;
	}

	/*re-estimate the projection matrix*/
	std::deque<pt_pair_s> pts; 
	float Pmat[3][4];
	float cam_dir[3];
	float error;

	for(int i_vol = 0; i_vol<particle.vol_particle_list.size();++i_vol){
		volParticle_s &vol = particle.vol_particle_list[i_vol];
		for(int i_node = 0; i_node<vol.tan_particle_list.size(); ++i_node){
			tanParticle_s &tan=vol.tan_particle_list[i_node];
			for(int i_pair= 0; i_pair<tan.pt_pairs.size();++i_pair){
				pts.push_back(tan.pt_pairs[i_pair]);
			}
		}
	}

	projector::estimatePandDir(pts,Pmat,cam_dir,error);

	for(int row =0; row<3;++row){
		memcpy(particle.P[row],Pmat[row],sizeof(float)*4);
	}
	memcpy(particle.cam_dir,cam_dir,sizeof(float)*3);

	//window.drawP(particle.P);
	//window.drawRootPtc(particle);

	/*normalize the feature response*/
#ifdef	AREA_NORMALIZE
	if(particle.bb_size!=0){
		float coef = 1.0f/((float)particle.bb_size);
		for(int i_vol = 0; i_vol<particle.vol_particle_list.size();++i_vol){
			volParticle_s &vol = particle.vol_particle_list[i_vol];
			for(int i_node = 0; i_node<vol.tan_particle_list.size(); ++i_node){
				vol.tan_particle_list[i_node].score*=coef;
				for(int i_el = 0; i_el<vol.tan_particle_list[i_node].response.size();++i_el){
					vol.tan_particle_list[i_node].response[i_el]*=coef;
				}/*i_el*/
			}/*i_node*/
		}/*i_vol*/
	}
#endif




	if(template_name !=NULL){
		/*save the template image*/
		//cv::Mat img;
		//img = cvCreateMat(image.sx,image.sy,CV_8UC1);
		//for( int row = 0; row<image.sx; ++row){
		//	for(int col = 0; col<image.sy; ++col){
		//		int val= (p_template[col*image.sx + row]/1.0f*255);
		//		img.at<unsigned char>(row,col)=val;
		//	}
		//}
		//std::string name(template_name);
		//imwrite(name,img);

		IplImage *pImg = cvCreateImage(cv::Size(image.sy,image.sx),IPL_DEPTH_8U,1);
		for( int row = 0; row<image.sx; ++row){
			for(int col = 0; col<image.sy; ++col){
				int val= (p_template[col*image.sx + row]/1.0f*255);
				//		img.at<unsigned char>(row,col)=val;
				pImg->imageData[col + row*image.sy]=255-val;
			}
		}
		cvSaveImage(template_name,pImg);
		cvReleaseImage(&pImg);
		//std::string name(template_name);
		//imwrite(name,img);


		free(p_template);
	}
}

void rootNode::test(float P[3][4],float cam_dir[3],	rootParticle_s &particle){
	particle.or_index_list.clear();
	particle.vol_particle_list.clear();

	window.setBackground(image.sy,image.sx,image.img_r,image.img_g,image.img_b);
	window.drawP(P);
	particle.score=0;
	for( int i_node = 0; i_node<this->vol_OR_list.size(); ++i_node){
		std::vector<int> deform_list;
		volParticle_s ptc;
		int or_index;
		float score;
		this->vol_OR_list[i_node].test(P,cam_dir,score,or_index,ptc);
		particle.score +=score;
		particle.or_index_list.push_back(or_index);
		particle.vol_particle_list.push_back(ptc);
	}

#ifdef AREA_NORMALIZE
	if(particle.bb_size!=0&&particle.score!=0){
		particle.score/=((float)particle.bb_size);;
	}
#endif

}


bool rootNode::initialRootNode(const char *path, std::vector<int> node_index_list, const char* weight_file_name){

	if(!vol_OR_list.empty())	this->vol_OR_list.clear();
	if(!vol_AND_list.empty())	this->vol_AND_list.clear();
	if(!tan_OR_list.empty())	this->tan_OR_list.clear();
	float z0=0.0f; //total complexity;
	FILE *p_weight_file;
	if(weight_file_name ==NULL){
		p_weight_file = NULL;
		z0 = 0.0f;
	}else{
		p_weight_file = fopen(weight_file_name,"rb");
#ifdef COMPLEXITY_NORMALIZE
		z0 = fread(&z0,sizeof(float),1,p_weight_file);
#endif
	}

	char root_path[1000];
	for(int i_root = 0; i_root<node_index_list.size(); ++i_root){
		sprintf(&root_path[0],"%s/tree_%02d",path,node_index_list[i_root], node_index_list[i_root]);
		insertVolORNode(const_cast<const char*>(root_path),node_index_list[i_root],p_weight_file);
	}
	if(p_weight_file!=NULL)	fclose(p_weight_file);
	//update the total complexity penalty to all terms
	for(int i_tan = 0; i_tan<tan_OR_list.size();++i_tan){
		//		tan_OR_list[i_tan].complexity_weight +=z0;
		tan_OR_list[i_tan].complexity_weight =0;
	}


	// read data X from file
	// read data from file
	const char* file_name = "D:/Projects/AoT/testCode/detection/frame_pts_v2.csv";
	FILE *p_file = fopen(file_name,"r");
	if( p_file == NULL){
		const char* file_name_2 = "/home/wzhu/Documents/CarAoT/testCode/detection/frame_pts_v2.csv";
		p_file = fopen(file_name_2,"r");

		if(p_file==NULL){		
			printf("Cannot open file : %s  NOR: %s\n",file_name, file_name_2);
			return false;
		}
	}
	// here we know that there are 28 points in the frame_pts_v2.csv
	printf("reading csv file in rootNode \n");
	for(int i = 0; i<28; ++i){
		fscanf(p_file,"%f,%f,%f",&X[i][0],&X[i][1],&X[i][2]);
		//X[i][3]=1;
	}
	fclose(p_file);



	/*set up tan index to feature index lookup table*/
#ifdef COMPLEXITY_NORMALIZE
	int i_feature =1;
#else
	int i_feature =0;
#endif
	for(int i_tan =0; i_tan<this->tan_OR_list.size(); ++i_tan){
		f_table_tan.push_back(tan_OR_list[i_tan].getID());
		f_table_feature.push_back(i_feature);
		i_feature+=(int)tan_OR_list[i_tan].tan_shape.line_list.size();
#ifdef COMPLEXITY_NORMALIZE
		i_feature ++;
#endif
#ifdef USE_COLOR
		i_feature+=30;
#endif
#ifdef USE_FLAT
		i_feature+=10;
#endif
		printf("id: %d, ind: %d\n",f_table_tan.back(),f_table_feature.back());
	}
	return true;
}

int rootNode::countFeatureDimension(){
	int n_stroke = 0;
	for(int i_tan = 0; i_tan<this->tan_OR_list.size(); ++i_tan){
		n_stroke+=tan_OR_list[i_tan].tan_shape.line_list.size();

#ifdef USE_COLOR
		n_stroke+=30; /*color histogram*/
#endif
#ifdef USE_FLAT
		n_stroke+=10;
#endif
	}
#ifdef COMPLEXITY_NORMALIZE
	n_stroke +=tan_OR_list.size();
	n_stroke ++; // for the total complexity term;
#endif
	return n_stroke;
}






bool rootNode::insertVolORNode(const char* vol_path, int tree_id,FILE *p_weight_file){
	char mat_file_name[1000];

	sprintf(mat_file_name,"%s/solTree_%02d_itr%02d.mat",vol_path,tree_id,5);
	MATFile *p_file = matOpen(mat_file_name,"r");
	if( p_file == NULL){
		printf("cannot open matlab file %s, will return; \n", mat_file_name);
		return false;
	}

	mxArray *p_tree_ensemble = matGetVariable(p_file,"resTree");
	volOR tmp_vol_or_node;
	this->vol_OR_list.push_back(tmp_vol_or_node);
	/*data to initialize current node*/
	vol_OR_list.back().id = tree_id;

	int n_tree = mxGetNumberOfElements(p_tree_ensemble);
	printf("number of trees in %s  is %d\n",mat_file_name,n_tree);
	/*initialize it's child*/
	for( int i_tree =1; i_tree<=n_tree; ++i_tree){
		const mxArray *p_tree = mxGetCell(p_tree_ensemble,i_tree-1);/*i_tree corresponds to folder cluster_%d*/
		if(this->insertVolANDNode(p_tree,vol_path,i_tree,tree_id,p_weight_file)){
			/*hook the child*/	
			//this->vol_OR_list.back().children.push_back(vol_AND_list.size());
			volAND *ptr = &(vol_AND_list.back());
			this->vol_OR_list.back().children.push_back(ptr);
		}
	}
	mxDestroyArray(p_tree_ensemble);
	matClose(p_file);
	return true;
}



bool rootNode::insertVolANDNode(const mxArray *p_tree, const char* vol_path, int i_tree, int vol_id,FILE *p_weight_file){
	char tree_path[1000];
	sprintf(&tree_path[0],"%s/itrEM%d/cluster_%d",vol_path,5,i_tree);
	const mxArray *p_term_layer= mxGetField(p_tree,0,"termLayer");
	/*fill in data for vol_AND node*/
	volAND tmp_vol_and;
	vol_AND_list.push_back(tmp_vol_and);
	volAND &current_node = vol_AND_list.back();
	current_node.id=vol_id*1000 + i_tree;/*cluster id*/


	/*initialize tan_OR node */
	int n_layer = mxGetM(p_term_layer);
	for(int i_layer = 0; i_layer < n_layer; ++i_layer){
		const mxArray *p_layer=mxGetCell(p_term_layer,i_layer);
		int n_node = mxGetN(p_layer);
		if (n_node==0) continue;
		printf(" %d nodes in layer %d \n",n_node,i_layer);
		for( int i_node = 0 ; i_node<n_node; ++i_node){
			if(this->insertTanORNode(p_layer,i_node,i_layer,tree_path,p_weight_file)){
				/*hook tan_OR node*/
				//vol_AND_list.back().children.push_back(tan_OR_list.size());
				tanOR *ptr=&(tan_OR_list.back());
				vol_AND_list.back().children.push_back(ptr);
			}
		}
	}
	return true;
}


bool rootNode::insertTanORNode(const mxArray *p_layer,int i_node, int i_layer, const char* tree_path,FILE *p_weight_file){
	char tan_path[1000];
	sprintf(&tan_path[0],"%s/tan_%d_%d",tree_path,i_layer+1,i_node+1);
	/*check if an emppty structure*/
	const mxArray *p_empty=mxGetField(p_layer,i_node,"R");
	if((void*)p_empty==NULL){
		printf("find an empty node in layer %d, node %d.\n",i_layer,i_node);
		return false;
	}

	float R[9]; /*columnwise, like the data in matlab*/
	float T[3];
	float swt;

	/*read the R T swt for tan node initialization*/
	char tan_param_file_name[1000];
	sprintf(&tan_param_file_name[0],"%s/rootParam.mat",tan_path);
	MATFile *tan_param = matOpen(tan_param_file_name,"r");
	if( tan_param ==NULL){
		printf("cannot open tan param file %s. Will return. \n",tan_param_file_name);
		return false;
	}
	mxArray *field_R = matGetVariable(tan_param,"R");
	float * p_r=(float*)mxGetPr(field_R);
	for( int i_digit = 0; i_digit<9; ++ i_digit) R[i_digit]=p_r[i_digit];
	mxDestroyArray(field_R);

	mxArray *field_t = matGetVariable(tan_param,"T");
	float * p_t=(float*)mxGetPr(field_t);
	for( int i_digit = 0; i_digit<3; ++ i_digit) T[i_digit]=-p_t[i_digit]; // minus sign is due to en error in matlab code
	mxDestroyArray(field_t);

	mxArray *field_swt = matGetVariable(tan_param,"swt");
	swt = mxGetScalar(field_swt);
	mxDestroyArray(field_swt);

	matClose(tan_param);

	tanOR tmp_tan_or;
	this->tan_OR_list.push_back(tmp_tan_or);
	tanOR &current_tan=tan_OR_list.back();
	current_tan.initialTanORNode(p_layer,R,T,swt,tan_path,tan_OR_list.size(),i_layer,i_node,p_weight_file);
	return true;
}


bool rootNode::updateParticleList(rootParticle_s & particle){

	if( this->root_particle_list.empty()){
		root_particle_list.push_back(particle);
		return true;
	}
	std::list<rootParticle_s>::iterator itr = root_particle_list.begin();
	for(;itr!=root_particle_list.end(); ++itr){
		if(particle.score>=itr->score){
			root_particle_list.insert(itr,particle);
			if(root_particle_list.size()>PR_QUEUE_SIZE){
				root_particle_list.pop_back();
			}
			return true;
		}
	}
	if(root_particle_list.size()<PR_QUEUE_SIZE){
		root_particle_list.push_back(particle);
		return true;
	}
	return false;

}



void rootNode::lookAroundV2(float srcP[3][4],rootParticle_s &particle){
	double rot[3][3];
	double trans[3];
	double cparam[3][4];
	float cam_dir[3];
	float singleP[3][4];
	rootParticle_s tmpParticle;


	double    factor;
	double    a, b, c;
	double    a1, b1, c1;
	double    a2, b2, c2;
	double	  trans1[3];
	double	  trans2[3];
	double    ma = 0.0, mb = 0.0, mc = 0.0;
	double    transm[3];
	double    combo[3][4];
	double    score, max_score;
	double	  cparam2[3][4];
	double	  cparam1[3][4];
	int       t1, t2, t3;
	int		  tx, ty, tz;
	int       s1 = 0, s2 = 0, s3 = 0;
	int sx=0, sy=0,sz=0;
	int mdf = 0;


	projector::decomposeP(srcP,rot,NULL,trans,cparam);
	subARGetAngle( rot, &a, &b, &c );
	/*to verify a b c and rot is reversible*/
	subARGetNewMatrix( a, b, c, trans, NULL, cparam, combo );
	for(int row = 0; row<3; ++row){
		for(int col =0; col<4; ++col){
			singleP[row][col]=(float)(combo[row][col]);
		}
	}
	projector::cam_direction(singleP,cam_dir);	
	window.cleanImage();
	window.show(1);
	window.drawP(singleP);

	/*initial parameters */
	a2 = a;	b2 = b;	c2 = c;
	memcpy(trans2,trans,sizeof(double)*3);
	memcpy(cparam2,cparam,sizeof(double)*12);
	ma = a; mb = b ; mc = c;
	factor = 5.0*MD_PI/180.0;
	double factor_trans= 0.1;
	double factor_f = 0.1;
	this->test(singleP,cam_dir,tmpParticle);
	max_score = tmpParticle.score;
	for(int j = 0; j < 5; j++ ) {

		s1 = 0; s2 =0; s3=0;
		/*block 2, the three angles*/
		for(t1=-1;t1<=1;t1++) {	for(t2=-1;t2<=1;t2++) {	for(t3=-1;t3<=1;t3++) {
			a1 = a2 + factor*t1;
			b1 = b2 + factor*t2;
			c1 = c2 + factor*t3;


			subARGetNewMatrix( a1, b1, c1, trans2, NULL, cparam2, combo );
			for(int row = 0; row<3; ++row){
				for(int col =0; col<4; ++col){
					singleP[row][col]=(float)(combo[row][col]);
				}
			}
			projector::cam_direction(singleP,cam_dir);	
			this->test(singleP,cam_dir,tmpParticle);
			if( tmpParticle.score >= max_score) {
				max_score = tmpParticle.score;
				ma = a1;mb = b1;mc = c1;
				s1 = t1; s2 = t2; s3 = t3;
			}

		}	}	}/*t1 t2 t3*/
		if( s1 == 0 && s2 == 0 && s3 == 0 ) factor *= 0.5;
		a2 = ma; b2 = mb; c2 = mc;

		/*block 3, the camera center*/
		sx = 0; sy = 0 ;sz=0;		mdf = 0;
		for(int df = -1; df<=1; ++df){
		for(tx=-1;tx<=1;tx++) { for(ty=-1;ty<=1;ty++) {	for(tz=-1;tz<=1;tz++) {
			trans1[0]=trans2[0] + factor_trans*trans[0]*tx;
			trans1[1]=trans2[1] + factor_trans*trans[1]*ty;
			trans1[2]=trans2[2] + factor_trans*trans[2]*tz;

			memcpy(cparam1,cparam2,sizeof(double)*12);
			cparam1[0][0]*=(1+df*factor_f);
			cparam1[1][1]*=(1+df*factor_f);

			subARGetNewMatrix( a2, b2, c2, trans1, NULL, cparam1, combo );
			for(int row = 0; row<3; ++row){
				for(int col =0; col<4; ++col){
					singleP[row][col]=(float)(combo[row][col]);
				}
			}
			projector::cam_direction(singleP,cam_dir);
			this->test(singleP,cam_dir,tmpParticle);

			if( tmpParticle.score >= max_score) {
				max_score =tmpParticle.score;
				memcpy(transm,trans1,sizeof(double)*3);
				sx = tx; sy = ty; sz = tz;
				memcpy(cparam,cparam1,sizeof(double)*12);
				mdf = df;
			}	
		}	}	}/*tx ty tz*/
		}/*f*/
		if( sx == 0 && sy == 0 && sz == 0 ) factor_trans *= 0.5;
		memcpy(trans2,transm,sizeof(double)*3);
		if (mdf==0) factor_f*=0.5;
		memcpy(cparam2,cparam,sizeof(double)*12);
	}/*for j*/

	subARGetRot( ma, mb, mc, rot );
	subARGetNewMatrix(ma, mb, mc, transm, NULL, cparam, combo );
	for(int row = 0; row<3; ++row){
		for(int col =0; col<4; ++col){
			singleP[row][col]=(float)(combo[row][col]);
		}
	}
	projector::cam_direction(singleP,cam_dir);
	for(int row = 0; row<3; ++row){
		for(int col =0; col<4; ++col){
			particle.P[row][col]=(float)(combo[row][col]);
		}
		particle.cam_dir[row]= cam_dir[row];
	}
	this->test(singleP,cam_dir,particle);
	window.cleanImage();
	window.drawP(singleP);

}



void rootNode::lookAround(float srcP[3][4],rootParticle_s &particle){
	double rot[3][3];
	double trans[3];
	double cparam[3][4];
	float cam_dir[3];
	float singleP[3][4];
	//double doubleP[3][4];
	rootParticle_s tmpParticle;


	double    factor;
	double    a, b, c;
	double    a1, b1, c1;
	double    a2, b2, c2;
	double	  trans1[3];
	double	  trans2[3];
	double    ma = 0.0, mb = 0.0, mc = 0.0;
	double    transm[3];
	double    combo[3][4];
	double    score, max_score;
	double	  cparam2[3][4];
	double	  cparam1[3][4];
	int       t1, t2, t3;
	int		  tx, ty, tz;
	int       s1 = 0, s2 = 0, s3 = 0;
	int sx=0, sy=0,sz=0;
	int mdf = 0;

	//window.cleanImage();
	//window.drawP(srcP);
	projector::decomposeP(srcP,rot,NULL,trans,cparam);

	subARGetAngle( rot, &a, &b, &c );
	subARGetNewMatrix( a, b, c, trans, NULL, cparam, combo );
	for(int row = 0; row<3; ++row){
		for(int col =0; col<4; ++col){
			singleP[row][col]=(float)(combo[row][col]);
		}
	}
	window.cleanImage();
	window.show(1);
	window.drawP(singleP);





	a2 = a;
	b2 = b;
	c2 = c;
	memcpy(trans2,trans,sizeof(double)*3);
	memcpy(cparam2,cparam,sizeof(double)*12);
	factor = 10.0*MD_PI/180.0;
	double factor_trans= 0.1;
	double factor_f = 0.1;
	max_score = -1000000000.0;
	for(int j = 0; j < 5; j++ ) {
		for(int df = -1; df<=1; ++df){
			memcpy(cparam1,cparam2,sizeof(double)*12);
			cparam1[0][0]*=(1+df*factor_f);
			cparam1[1][1]*=(1+df*factor_f);

			for(t1=-1;t1<=1;t1++) {	for(t2=-1;t2<=1;t2++) {	for(t3=-1;t3<=1;t3++) {
				a1 = a2 + factor*t1;
				b1 = b2 + factor*t2;
				c1 = c2 + factor*t3;
				for(tx=-1;tx<=1;tx++) { for(ty=-1;ty<=1;ty++) {	for(tz=-1;tz<=1;tz++) {
					trans1[0]=trans2[0] + factor_trans*trans[0]*tx;
					trans1[1]=trans2[1] + factor_trans*trans[1]*ty;
					trans1[2]=trans2[2] + factor_trans*trans[2]*tz;
					subARGetNewMatrix( a1, b1, c1, trans1, NULL, cparam1, combo );
					for(int row = 0; row<3; ++row){
						for(int col =0; col<4; ++col){
							singleP[row][col]=(float)(combo[row][col]);
						}
					}
					projector::cam_direction(singleP,cam_dir);

					this->test(singleP,cam_dir,tmpParticle);
					score = tmpParticle.score;
					if( score >= max_score) {
						max_score =score;
						ma = a1;
						mb = b1;
						mc = c1;
						s1 = t1; s2 = t2; s3 = t3;


						memcpy(transm,trans1,sizeof(double)*3);
						memcpy(cparam,cparam1,sizeof(double)*12);
						mdf = df;

						sx = tx; sy = ty; sz = tz;

						window.cleanImage();
						window.drawP(singleP);
					} /*score > max_score*/
				}	}   }
			}	}	}
		}/*df*/
		if( s1 == 0 && s2 == 0 && s3 == 0 ) factor *= 0.5;
		if( sx == 0 && sy == 0 && sz == 0 ) factor_trans *= 0.5;
		if( mdf ==0) factor_f*=0.5;
		a2 = ma;
		b2 = mb;
		c2 = mc;

		memcpy(trans2,transm,sizeof(double)*3);
		memcpy(cparam2,cparam,sizeof(double)*12);
	}/*for j*/

	subARGetRot( ma, mb, mc, rot );
	subARGetNewMatrix(ma, mb, mc, transm, NULL, cparam, combo );
	for(int row = 0; row<3; ++row){
		for(int col =0; col<4; ++col){
			singleP[row][col]=(float)(combo[row][col]);
		}
	}
	projector::cam_direction(singleP,cam_dir);
	for(int row = 0; row<3; ++row){
		for(int col =0; col<4; ++col){
			particle.P[row][col]=(float)(combo[row][col]);
		}
		particle.cam_dir[row]= cam_dir[row];
	}
	this->test(singleP,cam_dir,particle);
	window.cleanImage();
	window.drawP(singleP);

}

bool::rootNode::release(){
	int n_node = this->tan_OR_list.size();
	while(!tan_OR_list.empty()){
		tan_OR_list.back().release();
		tan_OR_list.pop_back();
	}
	return true;
}




bool rootNode::alignFeature(rootParticle_s &root_particle,float *p_feature_vec){
	/*this function assumes memory for p_feature_vec is alreay allocated, and is set to zero*/
	int num_segments,total_num_segments;
	total_num_segments =0;
	int i_dim = 0; /*feature dimension index*/
	for(int i_vol = 0; i_vol<root_particle.vol_particle_list.size(); ++i_vol){
		volParticle_s &vol = root_particle.vol_particle_list[i_vol];
		if(vol.tan_particle_list.size()==0) continue;
		for(int i_tan = 0; i_tan<vol.tan_particle_list.size(); ++i_tan){
			int id = vol.tan_particle_list[i_tan].id;
			for(int i = 0; i<f_table_tan.size(); ++i){
				if(id!=f_table_tan[i]) continue;
				/*found the correct entry, begin to copy response*/
				//printf("align feature id %d, itan %d, ivol %d,n_elem:%d \n",id,i_tan,i_vol,vol.tan_particle_list[i_tan].response.size());
				if(vol.tan_particle_list[i_tan].arg_max_deformation==-1) continue;

				i_dim = f_table_feature[i];
				num_segments= 0;
				for(int j =0; j<vol.tan_particle_list[i_tan].response.size(); ++j){
					p_feature_vec[i_dim++]=vol.tan_particle_list[i_tan].response[j];
#ifdef COMPLEXITY_NORMALIZE
					num_segments+=vol.tan_particle_list[i_tan].line_length[j]*2+1;
#endif
				}

#ifdef COMPLEXITY_NORMALIZE
				// go back to the first dimension, and fill in the complexity term
				p_feature_vec[i_dim++]=num_segments;
				total_num_segments+=num_segments;
#endif

#ifdef USE_COLOR
				/*add color feature*/
				for(int i_bin = 0; i_bin<30; ++i_bin){
					p_feature_vec[i_dim++]=vol.tan_particle_list[i_tan].color_feature[i_bin];
				}
#endif

#ifdef USE_FLAT
				for(int i_bin = 0; i_bin<10; ++i_bin){
					p_feature_vec[i_dim++]=vol.tan_particle_list[i_tan].flat_feature[i_bin];
				}
#endif

				break;
			}// for i
		}// for i_tan
	}// for i_vol
	//add the z0 term, or total complexity term;
#ifdef COMPLEXITY_NORMALIZE
	// go back to the first dimension, and fill in the complexity term
	p_feature_vec[0]=total_num_segments;
#endif			


	/*following code are for debug only*/
	//printf("feautre:\n");
	//for(int i = 0; i<140; ++i)
	//	printf("%f\t",p_feature_vec[i]);
	//printf("\n");

	return true;
}
